#pragma once

#include <array>
#include <cmath>
#include <algorithm>

#include "../utils/misc.h"

#define MANANO_PI 3.14159265358979323846264338327950288

namespace mdtx::online::impl
{
    template <typename T>
    class highpass
    {
    public:
        T a, x0{}, y{};
        bool init{};

        /**
         * @brief Construct a new highpass object
         *
         * @param alpha filter factor, equals to RC / (RC + delta_T), where RC is time constant
         */
        explicit highpass(T alpha)
            : a(alpha)
        {
        }

        void insert_impl(T x)
        {
            if (init)
                [[likely]]
            {
                y = a * (y + x - x0);
            }
            else
                [[unlikely]]
            {
                y = x;
                init = true;
            }
            x0 = x;
        }

        void reset_impl()
        {
            init = false;
            x0 = y = 0;
        }
    };

    template <typename T>
    class lowpass
    {
    public:
        T a, y{};
        kahan<T> acc_y{};
        bool init{};

        /**
         * @brief Construct a new lowpass object
         *
         * @param alpha filter factor, equals to delta_T / (RC + delta_T), hwere RC is time constant
         */
        explicit lowpass(T alpha)
            : a(alpha)
        {
        }

        void insert_impl(T x)
        {
            if (init)
                [[likely]]
            {
                acc_y(y, a * (x - y));
            }
            else
                [[unlikely]]
            {
                y = x;
                init = true;
            }
        }

        void reset_impl()
        {
            y = 0;
            acc_y.reset();
        }
    };

    template <typename T>
    class cusum_filter
    {
    public:
        T pos{}, neg{}, thres;
        int state{};

        explicit cusum_filter(T thres)
            : thres(thres)
        {
        }

        void insert_impl(T st, T drift = 0)
        {
            pos = std::max<T>(pos + st + drift, 0);
            neg = std::min<T>(neg + st - drift, 0);
            if (pos > thres)
            {
                state = 1;
                pos = 0;
            }
            else if (neg < -thres)
            {
                state = -1;
                neg = 0;
            }
            else
            {
                state = 0;
            }
        }

        void reset_impl()
        {
            pos = neg = 0;
            state = 0;
        }
    };

    template <typename T>
    class ladder4pole
    {
    public:
        T cut, res;
        std::array<T, 4> stage{}, delay{};
        T p, k, t1, t2;

        ladder4pole(T cutoff_hz, T resonance_hz, T smplrate)
            : cut(2 * cutoff_hz / smplrate)
        {
            p = cut * (1.8 - 0.8 * cut);
            k = 2.0 * std::sin(cut * MANANO_PI / 2) - 1;
            t1 = (1 - p) * 1.386249;
            t2 = 12 + t1 * t1;

            res = resonance_hz * (t2 + 6 * t1) / (t1 - 6 * t1);
        }

        void insert_impl(T x)
        {
            x = x - res * stage[3];

            stage[0] = x * p + delay[0] * p - k * stage[0];
            stage[1] = stage[0] * p + delay[1] * p - k * stage[1];
            stage[2] = stage[1] * p + delay[2] * p - k * stage[2];
            stage[3] = stage[2] * p + delay[3] * p - k * stage[3];

            stage[3] -= (stage[3] * stage[3] * stage[3]) / 6;

            delay[0] = x;
            delay[1] = stage[0];
            delay[2] = stage[1];
            delay[3] = stage[2];
        }

        void reset_impl()
        {
            stage.fill(0);
            delay.fill(0);
        }
    };
}
